#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
RRT基础模块

此模块提供RRT算法的基础实现。
"""

import numpy as np
import time

from .node import Node
from .tree import Tree


class RRTBase:
    """
    RRT（快速探索随机树）算法的基类。
    """
    
    def __init__(self, collision_checker, config):
        """
        使用碰撞检查器和配置初始化RRT基类。
        
        参数:
            collision_checker: 碰撞检查器对象
            config (dict): 配置字典
        """
        self.collision_checker = collision_checker
        self.config = config
        
        # 提取常用的配置参数
        self.goal_bias = config.get("goal_bias", 0.1)
        self.max_iterations = config.get("max_iterations", 5000)
        self.goal_tolerance = config.get("goal_tolerance", 5.0)
        
        # 初始化状态
        self.tree = None
        self.start_node = None
        self.goal_node = None
        self.goal_reached = False
        self.iterations = 0
        self.path = None
        
        # 统计信息和元数据
        self.planning_time = 0
        self.nearest_to_goal = None
        
    def plan(self, start, goal, max_iterations=None, goal_tolerance=None, plotter=None):
        """
        使用RRT规划从起点到目标的路径。
        
        参数:
            start (tuple): 起始位置 (x, y)
            goal (tuple): 目标位置 (x, y)
            max_iterations (int, optional): 最大迭代次数
            goal_tolerance (float, optional): 目标达成容差
            plotter (Plotter, optional): 用于可视化的绘图器
            
        返回:
            tuple: (path, info) 其中path是点的列表，info是
                  包含额外信息的字典
        """
        # 如果提供，则覆盖参数
        if max_iterations is not None:
            self.max_iterations = max_iterations
        if goal_tolerance is not None:
            self.goal_tolerance = goal_tolerance
            
        # 初始化规划
        start_time = time.time()
        self._init_planning(start, goal)
        
        # 如果提供了绘图器，则初始化可视化
        if plotter:
            plotter.init_planning(self.tree, start, goal)
        
        # 主规划循环
        for i in range(self.max_iterations):
            # 更新迭代计数
            self.iterations = i + 1
            
            # 采样随机点
            if np.random.random() < self.goal_bias:
                # 以goal_bias的概率采样目标
                x_rand, y_rand = goal
            else:
                # 以(1 - goal_bias)的概率采样随机点
                x_rand, y_rand = self._sample_free()
            
            # 在树中找到最近的节点
            nearest_node = self.tree.get_nearest_node(x_rand, y_rand)
            
            # 朝随机点方向引导
            x_new, y_new = self._steer(nearest_node.position, (x_rand, y_rand))
            
            # 检查到新点的路径是否无碰撞
            if not self._check_segment_collision(nearest_node.position, (x_new, y_new)):
                # 创建并将新节点添加到树中
                cost = nearest_node.cost + self._compute_distance(nearest_node.position, (x_new, y_new))
                new_node = Node(x_new, y_new, nearest_node, cost)
                self.tree.add_node(new_node)
                
                # 检查是否已达到目标
                distance_to_goal = self._compute_distance((x_new, y_new), goal)
                
                # 如需要，更新最接近目标的节点
                if self.nearest_to_goal is None or distance_to_goal < self._compute_distance(self.nearest_to_goal.position, goal):
                    self.nearest_to_goal = new_node
                
                if distance_to_goal <= self.goal_tolerance:
                    # 目标已达成
                    self.goal_reached = True
                    self.goal_node = new_node
                    break
            
            # 如果提供了绘图器，则更新可视化
            if plotter and i % 10 == 0:  # 每10次迭代更新一次，以避免过度渲染
                plotter.update_planning(self.tree, self.nearest_to_goal, self.goal_node)
        
        # 规划后提取路径
        self.planning_time = time.time() - start_time
        self.path = self._extract_path()
        
        # 如果提供了绘图器，则进行最终可视化更新
        if plotter:
            plotter.update_planning(self.tree, self.nearest_to_goal, self.goal_node, self.path)
        
        # 返回路径和规划信息
        info = {
            "num_nodes": self.tree.size,
            "iterations": self.iterations,
            "planning_time": self.planning_time,
            "goal_reached": self.goal_reached,
            "nodes": self.tree.nodes,
            "goal_node": self.goal_node
        }
        
        return self.path, info
        
    def _init_planning(self, start, goal):
        """
        初始化规划过程。
        
        参数:
            start (tuple): 起始位置 (x, y)
            goal (tuple): 目标位置 (x, y)
        """
        # 创建根节点并初始化树
        self.start_node = Node(start[0], start[1])
        self.tree = Tree(self.start_node)
        
        # 重置状态
        self.goal_reached = False
        self.iterations = 0
        self.path = None
        self.nearest_to_goal = self.start_node
        self.goal_node = None
        
    def _sample_free(self):
        """
        在自由空间中采样随机点。
        
        返回:
            tuple: 采样点的(x, y)坐标
        """
        # 让地图采样随机点
        map_instance = self.collision_checker.map
        robot_radius = self.collision_checker.robot.radius
        
        # 尝试采样有效点
        point = map_instance.sample_valid_point(robot_radius)
        
        # 如果采样失败，使用地图边界采样
        if point is None:
            width, height = map_instance.get_dimensions()
            x = np.random.uniform(0, width)
            y = np.random.uniform(0, height)
            point = (x, y)
            
        return point
    
    def _steer(self, from_point, to_point):
        """
        从一个点向另一个点引导，遵守步长限制。
        
        参数:
            from_point (tuple): 起始点 (x, y)
            to_point (tuple): 目标点 (x, y)
            
        返回:
            tuple: 引导后的新点 (x, y)
        """
        # 使用机器人模型进行引导
        robot = self.collision_checker.robot
        return robot.steer(from_point, to_point)
    
    def _check_segment_collision(self, from_point, to_point, resolution=0.1):
        """
        检查两点之间的线段是否无碰撞。
        
        参数:
            from_point (tuple): 起始点 (x, y)
            to_point (tuple): 终点 (x, y)
            resolution (float): 沿线段进行碰撞检查的分辨率
            
        返回:
            bool: 若有碰撞则为True，若无碰撞则为False
        """
        # 计算线段长度和方向
        from_x, from_y = from_point
        to_x, to_y = to_point
        
        dx = to_x - from_x
        dy = to_y - from_y
        
        segment_length = np.sqrt(dx**2 + dy**2)
        
        # 要检查的中间点数量
        num_points = max(2, int(segment_length / resolution))
        
        # 沿线段以规则间隔检查碰撞
        for i in range(num_points):
            t = i / (num_points - 1)  # 插值参数 [0, 1]
            x = from_x + t * dx
            y = from_y + t * dy
            
            if self.collision_checker.check_collision(x, y):
                return True  # 检测到碰撞
        
        return False  # 无碰撞
    
    def _compute_distance(self, point1, point2):
        """
        计算两点之间的欧几里得距离。
        
        参数:
            point1 (tuple): 第一个点 (x, y)
            point2 (tuple): 第二个点 (x, y)
            
        返回:
            float: 欧几里得距离
        """
        x1, y1 = point1
        x2, y2 = point2
        return np.sqrt((x1 - x2)**2 + (y1 - y2)**2)
    
    def _extract_path(self):
        """
        规划后提取从起点到目标（或最接近目标的点）的路径。
        
        返回:
            list: 从起点到目标的(x, y)坐标列表
        """
        # 如果达到目标，提取到目标节点的路径
        if self.goal_reached and self.goal_node:
            return self.tree.get_path_to_node(self.goal_node)
        
        # 否则，提取到最接近目标的节点的路径
        if self.nearest_to_goal:
            return self.tree.get_path_to_node(self.nearest_to_goal)
        
        # 后备方案：空路径
        return []
    
    def compute_path_length(self, path):
        """
        计算给定路径的长度。
        
        参数:
            path (list): (x, y)坐标列表
            
        返回:
            float: 路径长度（欧几里得距离之和）
        """
        if not path or len(path) < 2:
            return 0.0
            
        length = 0.0
        for i in range(len(path) - 1):
            length += self._compute_distance(path[i], path[i + 1])
            
        return length 